#include "../Model/ModelProcessor.h"
#include "../Model/ModelProcessorWinter.h"
#include "../XMLTools.h"

#include <boost/lexical_cast.hpp>
#include <vector>
#include <map>
#include <set>

#include "MotionWriterXML.h"

using namespace MMM;

void MotionWriterXML::appendMotionNode(RapidXMLWriterNodePtr root, MotionPtr motion, const std::string &path) {
    RapidXMLWriterNodePtr motionNode = root->append_node(XML::NODE_MOTION);
    motionNode->append_attribute(XML::ATTRIBUTE_NAME, motion->getName().c_str());
    if (motion->isSynchronized()) motionNode->append_attribute("synchronized", "true");

    if (motion->getModel()) {
        //write Model
        std::string modelFileName = motion->getModel()->getFilename();
        if (!path.empty()) XML::makeRelativePath(XML::getPath(path), modelFileName);
        RapidXMLWriterNodePtr modelNode = motionNode->append_node(XML::NODE_MODEL)->append_attribute(XML::ATTRIBUTE_PATH, modelFileName);

        // write Modelprocessor
        if (motion->getModelProcessor()) motion->getModelProcessor()->appendDataXML(modelNode);
    }

    RapidXMLWriterNodePtr sensorsNode = motionNode->append_node("Sensors");
    for (auto sensor : motion->getPrioritySortedSensorData()) {
      sensor->appendSensorXML(sensorsNode);
    }
}

std::string MotionWriterXML::toXMLString(MotionPtr motion, const std::string &path) {
    RapidXMLWriterPtr writer(new RapidXMLWriter());
    RapidXMLWriterNodePtr root = writer->createRootNode(XML::MOTION_XML_ROOT);
    root->append_attribute(XML::ATTRIBUTE_VERSION, "2.0");
    appendMotionNode(root, motion, path);
    return writer->print(true);
}

std::string MotionWriterXML::toXMLString(MotionList motions, const std::string &path) {
    RapidXMLWriterPtr writer(new RapidXMLWriter());
    RapidXMLWriterNodePtr root = writer->createRootNode(XML::MOTION_XML_ROOT);
    root->append_attribute(XML::ATTRIBUTE_VERSION, "2.0");
    std::set<std::string> motionNames;
    for (auto motion : motions) {
        if (motionNames.find(motion->getName()) != motionNames.end()) throw Exception::MMMException("Writing Motion cancelled, because duplicate of motion name + '" + motion->getName() + "' found. Motion names need to be unique!");
        motionNames.insert(motion->getName());
        appendMotionNode(root, motion, path);
    }
    return writer->print(true);
}

void MotionWriterXML::writeMotion(MotionPtr motion, const std::string &path) {
    std::ofstream file;
    file.open(path.c_str());
    file << toXMLString(motion, path);
    file.close();
}

void MotionWriterXML::writeMotion(MotionList motions, const std::string &path) {
    std::ofstream file;
    file.open(path.c_str());
    try {
        file << toXMLString(motions, path);
    } catch (Exception::MMMException &e) {
        MMM_ERROR << e.what() << std::endl;
    }
    file.close();
}

void MotionWriterXML::replaceAndWriteMotions(MMM::MotionPtr motion, MMM::MotionList motions, const std::string &motionFilePath) {
    writeMotion(Motion::replaceMotion(motion, motions), motionFilePath);
}
